1.1. Surface Matching : example on human scapula

import trimesh
import numpy as np
from scipy import optimize
import plot_mesh as pltm
import plotly.graph_objects as go
import pandas as pd
import gbu
fig = pltm.create_mesh3D(stl_file="scapula.stl", title="Scapula Left <br> stl file")
fig.show()

1.1.1. Define reference points on STL file

p_glene_stl = np.array([0.02515289854664963, 0.03122873596800735, 0.036798809060995745])
p_coracoide_stl = np.array(
    [0.026787610816186087, 0.05609095153197788, -0.0043762069034760384]
)
p_acromion_stl = np.array(
    [-0.008420164259823573, 0.07140986464971243, 0.018059532550748165],
)
fig = pltm.add_points(points=p_glene_stl, fig=fig, name="glène")
fig = pltm.add_points(points=p_coracoide_stl, fig=fig, name="coracoide")
fig = pltm.add_points(points=p_acromion_stl, fig=fig, name="acromion")
fig.show()

1.1.2. Scan points

df_scan = pd.read_pickle("data_scan.p")
df_scan
p_glene_scapula p_acromion_scapula p_coracoide_scapula p_scan_scapula
p3d p3d p3d p3d
x y z x y z x y z x y z
0 0.014046 0.010801 0.118422 -0.023197 -0.022039 0.086035 -0.002085 -0.037214 0.123242 0.015003 -0.004692 0.11943
1 0.014152 0.01073 0.118276 -0.023097 -0.022086 0.085978 -0.002091 -0.037263 0.123189 0.015018 -0.004673 0.119397
2 0.014185 0.010854 0.11838 -0.023238 -0.022144 0.086024 -0.002113 -0.037188 0.123366 0.015072 -0.004562 0.119343
3 0.014362 0.01094 0.118554 -0.023217 -0.022149 0.086317 -0.001946 -0.037163 0.123109 0.015048 -0.004463 0.119347
4 0.014379 0.010899 0.118494 -0.023224 -0.022023 0.086214 -0.001959 -0.036706 0.12319 0.015013 -0.004507 0.11929
... ... ... ... ... ... ... ... ... ... ... ... ...
79 NaN NaN NaN NaN NaN NaN NaN NaN NaN 0.015566 -0.001602 0.126646
80 NaN NaN NaN NaN NaN NaN NaN NaN NaN 0.01562 -0.001514 0.126536
81 NaN NaN NaN NaN NaN NaN NaN NaN NaN 0.015541 -0.001624 0.126567
82 NaN NaN NaN NaN NaN NaN NaN NaN NaN 0.015546 -0.001497 0.126532
83 NaN NaN NaN NaN NaN NaN NaN NaN NaN 0.015578 -0.001497 0.126524

84 rows × 12 columns

cols = set([cols[0] for cols in df_scan.columns])
fig = go.Figure()
for i, c in enumerate(cols):
    fig.add_trace(
        go.Scatter3d(
            x=df_scan[c].p3d.x,
            y=df_scan[c].p3d.y,
            z=df_scan[c].p3d.z,
            name=c,
            marker_size=2,
            mode="markers",
        )
    )
fig.show()

1.1.3. Create initial guess

p_glene_scan = np.array(df_scan["p_glene_scapula"].p3d.mean())
p_coracoide_scan = np.array(df_scan["p_coracoide_scapula"].p3d.mean())
p_acromion_scan = np.array(df_scan["p_acromion_scapula"].p3d.mean())

tri_point_stl = np.concatenate([p_glene_stl, p_coracoide_stl, p_acromion_stl]).reshape(
    -1, 3
)
tri_point_scan = np.concatenate(
    [p_glene_scan, p_coracoide_scan, p_acromion_scan]
).reshape(-1, 3)


def unpack(X):
    return X.reshape(2, 3)


def cost(X):
    Rsc2stl, Tsc2stl = unpack(X)
    tri_point_out = gbu.utils.apply_RT(tri_point_scan, Rsc2stl, Tsc2stl)
    dist = abs(tri_point_stl - tri_point_out)
    pen_pin_radius = 0
    res = dist - pen_pin_radius

    return res.flatten()


X0 = np.zeros(6)
sol = optimize.least_squares(
    cost,
    X0,
    method="lm",
    ftol=1.0e-12,
    xtol=1.0e-12,
    gtol=1.0e-10,
)

X_pre = sol.x
R_inital_guess, T_initial_guess = unpack(X_pre)
print(f"Initial guess solution: rvec={R_inital_guess}, tvec={T_initial_guess}")
Initial guess solution: rvec=[ 2.23440972 -0.14576349  0.55625112], tvec=[-0.02428643  0.12368749  0.09225085]

1.2. Surface match with full scan

  1. Gathering input datas

p_full_scan_sc = np.array(df_scan["p_scan_scapula"])

# load full scapula expand
scale = 1e-3  # convert to meter
mesh_scapula = trimesh.exchange.load.load("scapula.stl", type="stl")
mesh_scapula.vertices *= scale
  1. Optimization

def cost(X, p3d, mesh):
    Rsc2stl, Tsc2stl = unpack(X)
    p_stl = gbu.utils.apply_RT(p3d, Rsc2stl, Tsc2stl)
    dist = trimesh.proximity.signed_distance(mesh, p_stl)
    pen_pin_radius = 0
    res = dist - pen_pin_radius
    return res


sol = optimize.least_squares(
    cost,
    X_pre,
    method="lm",
    ftol=1.0e-12,
    xtol=1.0e-12,
    gtol=1.0e-10,
    args=(p_full_scan_sc, mesh_scapula),
)

R_sc2stl_sol, T_sc2stl_sol = unpack(sol.x)
print(f"Final solution: rvec={R_sc2stl_sol}, tvec={T_sc2stl_sol}")
---------------------------------------------------------------------------
KeyboardInterrupt                         Traceback (most recent call last)
/tmp/ipykernel_171/2376106266.py in <module>
      8 
      9 
---> 10 sol = optimize.least_squares(
     11     cost,
     12     X_pre,

/opt/conda/lib/python3.10/site-packages/scipy/optimize/_lsq/least_squares.py in least_squares(fun, x0, jac, bounds, method, ftol, xtol, gtol, x_scale, loss, f_scale, diff_step, tr_solver, tr_options, jac_sparsity, max_nfev, verbose, args, kwargs)
    922 
    923     if method == 'lm':
--> 924         result = call_minpack(fun_wrapped, x0, jac_wrapped, ftol, xtol, gtol,
    925                               max_nfev, x_scale, diff_step)
    926 

/opt/conda/lib/python3.10/site-packages/scipy/optimize/_lsq/least_squares.py in call_minpack(fun, x0, jac, ftol, xtol, gtol, max_nfev, x_scale, diff_step)
     60             # n squared to account for Jacobian evaluations.
     61             max_nfev = 100 * n * (n + 1)
---> 62         x, info, status = _minpack._lmdif(
     63             fun, x0, (), full_output, ftol, xtol, gtol,
     64             max_nfev, epsfcn, factor, diag)

/opt/conda/lib/python3.10/site-packages/scipy/optimize/_lsq/least_squares.py in fun_wrapped(x)
    813 
    814     def fun_wrapped(x):
--> 815         return np.atleast_1d(fun(x, *args, **kwargs))
    816 
    817     if method == 'trf':

/tmp/ipykernel_171/2376106266.py in cost(X, p3d, mesh)
      2     Rsc2stl, Tsc2stl = unpack(X)
      3     p_stl = gbu.utils.apply_RT(p3d, Rsc2stl, Tsc2stl)
----> 4     dist = trimesh.proximity.signed_distance(mesh, p_stl)
      5     pen_pin_radius = 0
      6     res = dist - pen_pin_radius

/opt/conda/lib/python3.10/site-packages/trimesh/proximity.py in signed_distance(mesh, points)
    275 
    276     # For all other triangles, resort to raycasting against the entire mesh
--> 277     inside = mesh.ray.contains_points(points[nonzero[~ontriangle]])
    278     sign = (inside.astype(int) * 2) - 1.0
    279 

/opt/conda/lib/python3.10/site-packages/trimesh/ray/ray_triangle.py in contains_points(self, points)
    189         """
    190 
--> 191         return contains_points(self, points)
    192 
    193 

/opt/conda/lib/python3.10/site-packages/trimesh/constants.py in timed(*args, **kwargs)
    144     def timed(*args, **kwargs):
    145         tic = now()
--> 146         result = method(*args, **kwargs)
    147         log.debug('%s executed in %.4f seconds.',
    148                   method.__name__,

/opt/conda/lib/python3.10/site-packages/trimesh/ray/ray_util.py in contains_points(intersector, points, check_direction)
     60 
     61     # cast a ray both forwards and backwards
---> 62     location, index_ray, c = intersector.intersects_location(
     63         np.vstack(
     64             (points[inside_aabb],

/opt/conda/lib/python3.10/site-packages/trimesh/ray/ray_triangle.py in intersects_location(self, ray_origins, ray_directions, **kwargs)
    101         (index_tri,
    102          index_ray,
--> 103          locations) = self.intersects_id(
    104              ray_origins=ray_origins,
    105              ray_directions=ray_directions,

/opt/conda/lib/python3.10/site-packages/trimesh/ray/ray_triangle.py in intersects_id(self, ray_origins, ray_directions, return_locations, multiple_hits, **kwargs)
     58         (index_tri,
     59          index_ray,
---> 60          locations) = ray_triangle_id(
     61              triangles=self.mesh.triangles,
     62              ray_origins=ray_origins,

/opt/conda/lib/python3.10/site-packages/trimesh/ray/ray_triangle.py in ray_triangle_id(triangles, ray_origins, ray_directions, triangles_normal, tree, multiple_hits)
    249     # get the plane origins and normals from the triangle candidates
    250     plane_origins = triangle_candidates[:, 0, :]
--> 251     if triangles_normal is None:
    252         plane_normals, triangle_ok = triangles_mod.normals(
    253             triangle_candidates)

KeyboardInterrupt: 

Let’see the results

p_full_scan_stl = gbu.utils.apply_RT(p_full_scan_sc, R_sc2stl_sol, T_sc2stl_sol)

fig = pltm.create_mesh3D(stl_file="scapula.stl", title="Scapula Left <br> stl file")

fig = pltm.add_points(
    points=p_full_scan_stl, fig=fig, name="scanned_points", marker_size=10
)

fig.show()